#include "tag_save_map/tag_save_map.h"
#include <regex>
#include <fstream>
#include <yaml-cpp/yaml.h>



void TagSaveMap::processTF(const std::vector<geometry_msgs::msg::TransformStamped>& transforms) {
    for (const auto& transform : transforms) {
        const std::string& child_frame = transform.child_frame_id;
        // 记录所有包含"tag"的子坐标系
        if (child_frame.find("tag") != std::string::npos) {
            std::lock_guard<std::mutex> lock(mutex_);
            tag_frames_.insert(child_frame);
        }
    }
}



void TagSaveMap::TFCallback()
{
    std::map<std::string, geometry_msgs::msg::Transform> transform_map;
    // 构建YAML结构
    YAML::Node yaml_node; 

    std::ifstream fin("/home/fwy/nav_ws/src/fishbot_apriltag_detect/tag_save_map/tag_tf_map.yaml");
    if (fin.is_open())
    {
        yaml_node = YAML::Load(fin);
        fin.close();
    }
    // 获取所有tag坐标系的变换
    {
        std::lock_guard<std::mutex> lock(mutex_);
        for (const auto& child_frame : tag_frames_) {
            try {
                auto transform = tf_buffer_->lookupTransform(
                    "map", 
                    child_frame,
                    tf2::TimePointZero
                );
                transform_map[child_frame] = transform.transform;
            } 
            catch (const tf2::TransformException& ex) {
                // RCLCPP_WARN(get_logger(), "无法获取 %s 的变换: %s", 
                //            child_frame.c_str(), ex.what());
            }
        }
    }


    for (const auto& [frame_name, transform] : transform_map) {
        YAML::Node transform_node;
        
        // 平移
        transform_node["translation"]["x"] = transform.translation.x;
        transform_node["translation"]["y"] = transform.translation.y;
        transform_node["translation"]["z"] = transform.translation.z;
        
        // 旋转
        transform_node["rotation"]["x"] = transform.rotation.x;
        transform_node["rotation"]["y"] = transform.rotation.y;
        transform_node["rotation"]["z"] = transform.rotation.z;
        transform_node["rotation"]["w"] = transform.rotation.w;
        yaml_node["header"] = "map";
        
        yaml_node[frame_name] = transform_node;
    }

    // 写入文件（同名坐标系会自动覆盖）
    std::ofstream fout("/home/fwy/nav_ws/src/fishbot_apriltag_detect/tag_save_map/tag_tf_map.yaml");
    fout << yaml_node;
    RCLCPP_INFO(get_logger(), "已保存 %zu 个tag变换到tags.yaml", transform_map.size());
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TagSaveMap>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
